#define CATCH_CONFIG_MAIN
#include <catch.hpp>

#include <random>
#include <ctime>
#include <iostream>

#include "../../MiniSpline/Basic/Matrix.h"
#include "../../MiniSpline/Basic/Vector3f.h"
#include "../../MiniSpline/Basic/Quaternion.h"
#include "../../MiniSpline/Basic/ODESolver.h"
#include "../../MiniSpline/Object/MiniCloud.h"
#include "../../MiniSpline/Object/MiniDiscreteCurve.h"
#include "../../MiniSpline/Object/MiniBSplineCurve.h"
#include "../../MiniSpline/Object/MiniBSplineSurface.h"

using namespace XYF;
using namespace std;

TEST_CASE("Test Vector")
{
    SECTION("Test comparison")
    {
        REQUIRE(Vector({1, 2, 3}) == Vector({1, 2, 3}));
        REQUIRE(Vector({1.5, 2.6, 3.8, 4.8, 5.7}) == Vector({1.5, 2.6, 3.8, 4.8, 5.7}));
        REQUIRE_FALSE(Vector({1.5, 2.6, 3.8, 4.8, 5.7}) == Vector({1, 2, 3}));
    }
    SECTION("Test initialization")
    {
        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        REQUIRE(v.size() == 4);

        Vector v2(v.size(), 1.1);
        REQUIRE(v2 == Vector({1.1, 1.1, 1.1, 1.1}));

        Vector v3 = v2;
        REQUIRE(v3 == Vector({1.1, 1.1, 1.1, 1.1}));
    }
    SECTION("Test evaluation")
    {
        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        Vector v2(4);
        v2 = v;
        REQUIRE(v2 == v);
    }
    SECTION("Test []")
    {
        double arr[] = {1.5, 2.6, 3.8, 4.7};

        // 测试 []
        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        for (int i = 0; i < v.size(); i++)
            REQUIRE(v[i] == arr[i]);

        // 测试取引用
        for (int i = 0; i < v.size(); i++)
            v[i] = i;
        for (int i = 0; i < v.size(); i++)
            REQUIRE(v[i] == i);
    }
    SECTION("Test +-*/")
    {
        double arr[] = {1.5, 2.6, 3.8, 4.7};
        double arr2[4];
        for (int i = 0; i < 4; i++)
            arr2[i] = arr[i] * 2;

        Vector v = Vector({1.5, 2.6, 3.8, 4.7});
        Vector v2 = v + v;
        for (int i = 0; i < v.size(); i++)
            REQUIRE(v2[i] == arr2[i]);

        Vector v3 = v - v;
        for (int i = 0; i < v.size(); i++)
            REQUIRE(v3[i] == 0);

        Vector v4 = v * 2;
        for (int i = 0; i < v.size(); i++)
            REQUIRE(v4[i] == arr2[i]);

        Vector v5 = v2 / 2;
        for (int i = 0; i < v.size(); i++)
            REQUIRE(v5[i] == arr[i]);

        Vector v6 = -v2;
        for (int i = 0; i < v.size(); i++)
            REQUIRE(v6[i] == -arr2[i]);
    }
    SECTION("Test times Matrix")
    {
        Vector v({1, 2, 3});
        Matrix A(1, 7, {1, 2, 3, 4, 5, 6, 7});
        Matrix B = v * A;
        REQUIRE(B == Matrix(3, 7, {1, 2, 3, 4, 5, 6, 7, 2, 4, 6, 8, 10, 12, 14, 3, 6, 9, 12, 15, 18, 21}));
    }
}

TEST_CASE("Test Matrix")
{
    SECTION("Test comparison")
    {
        REQUIRE(Matrix(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                Matrix(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        REQUIRE(Matrix(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                Matrix(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}));
        REQUIRE_FALSE(Matrix(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}) ==
                      Matrix(3, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9}));
    }
    SECTION("Test initialization")
    {
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        REQUIRE((A.row() == 3 && A.col() == 4));

        Matrix A2(4, 3, 1.1);
        REQUIRE(A2 == Matrix(4, 3, {1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1}));

        Matrix A3 = A2;
        REQUIRE(A3 == A2);
    }
    SECTION("Test evaluation")
    {
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        Matrix A2(3, 4);
        A2 = A;
        REQUIRE(A2 == A);
    }
    SECTION("Test ()")
    {
        double arr[] = {1.2, 2.5, 34, 4.8, 5.1, 6.9, 7.1, 8.2, 9.7, 10.5, 11.4, 12.2};

        // 测试取引用
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        for (int i = 0; i < A.row(); i++)
            for (int j = 0; j < A.col(); j++)
                A(i, j) = arr[i * 4 + j];
        for (int i = 0; i < A.row(); i++)
            for (int j = 0; j < A.col(); j++)
                REQUIRE((A(i, j) == arr[i * 4 + j]));

        REQUIRE(A.row() == 3);
        REQUIRE(A.col() == 4);
    }
    SECTION("Test +-*/")
    {
        Matrix A(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        Matrix A2 = A * 2;
        REQUIRE(A2 == Matrix(3, 4, {2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24}));

        Matrix A3 = A2 / 2;
        REQUIRE(A3 == A);

        Matrix A4 = A + A;
        REQUIRE(A4 == A2);

        Matrix A5 = A2 - A;
        REQUIRE(A5 == A);

        Vector v({1, 2, 3, 4});
        Vector v2 = A * v;
        REQUIRE(v2 == Vector({30, 70, 110}));

        Matrix B(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        Matrix A6 = A * B;
        REQUIRE(A6 == Matrix(3, 3, {70, 80, 90, 158, 184, 210, 246, 288, 330}));

        Matrix A7 = -A;
        REQUIRE((A7 + A) == Matrix(3, 4));
    }
}

TEST_CASE("Test Vector algorithms")
{
    SECTION("Sort")
    {
        Vector X({5, 2.0, 5.1, 9.3, 1.5, -5, 6.4, 3.9, 4.2});
        X.sort();
        REQUIRE(X == Vector({-5, 1.5, 2.0, 3.9, 4.2, 5, 5.1, 6.4, 9.3}));
    }
    SECTION("Sum")
    {
        Vector X({1, 2, 3, 4, 5, 6});
        REQUIRE(X.sum() == 21);
    }
    SECTION("Abs")
    {
        Vector X({-1, -2, -3, -4, -5, -6});
        Vector Y({1, 2, 3, 4, 5, 6});
        REQUIRE(X.abs() == Y);
    }
    SECTION("Sgn")
    {
        Vector X({-1, 2, -3, 4, -5, -6});
        REQUIRE(X.sgn() == Vector({-1, 1, -1, 1, -1, -1}));
    }
    SECTION("Norm")
    {
        Vector x({1, 2, 3, 4, 5, 6});
        REQUIRE(x.norm(0) == 6);
        REQUIRE(x.norm(1) == 21);
        REQUIRE(x.norm(2) == sqrt(91));
    }
    SECTION("Min-Max")
    {
        Vector x({1, 2, 33, -46, 5, 6});
        REQUIRE(x.min() == -46);
        REQUIRE(x.max() == 33);
        REQUIRE(x.maxId() == 3);
    }
    SECTION("To Matrix")
    {
        Vector x({1, 2, 33, -46, 5, 6});
        Matrix A1(1, 6, {1, 2, 33, -46, 5, 6});
        Matrix A2(6, 1, {1, 2, 33, -46, 5, 6});
        REQUIRE(A2 == x.toMatrix());
        REQUIRE(A1 == x.transpose());
    }
}

TEST_CASE("Test Matrix Operations")
{
    SECTION("Norm")
    {
        Matrix A(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        REQUIRE(A.norm(0) == 33);
        REQUIRE(A.norm(1) == 30);

        Approx target1 = Approx(A.norm(2)).epsilon(1e-10);
        REQUIRE(target1 == 25.462407436036393);
        Approx target2 = Approx(A.norm('f')).epsilon(1e-10);
        REQUIRE(target2 == 25.495097567963924);
    }
    SECTION("Condition")
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        Approx target = Approx(A.condition()).epsilon(1e-10);
        REQUIRE(target == 133.0);
    }
    SECTION("Determinant")
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        Approx target = Approx(A.det()).epsilon(1e-10);
        REQUIRE(target == -3);
    }
    SECTION("Operation")
    {
        Matrix A(4, 3, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        REQUIRE(A.upper() == Matrix(4, 3, {1, 2, 3, 0, 5, 6, 0, 0, 9, 0, 0, 0}));
        REQUIRE(A.lower() == Matrix(4, 3, {1, 0, 0, 4, 5, 0, 7, 8, 9, 10, 11, 12}));

        Matrix B(3, 4, {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12});
        REQUIRE(B.upper() == Matrix(3, 4, {1, 2, 3, 4, 0, 6, 7, 8, 0, 0, 11, 12}));
        REQUIRE(B.lower() == Matrix(3, 4, {1, 0, 0, 0, 5, 6, 0, 0, 9, 10, 11, 0}));

        REQUIRE(A.transpose() == Matrix(3, 4, {1, 4, 7, 10, 2, 5, 8, 11, 3, 6, 9, 12}));
    }
    SECTION("Inversion")
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        Matrix B = A.inverse();
        REQUIRE(B == Matrix(3, 3, {-2.0 / 3, -2.0 / 3, 1.0, -4.0 / 3, 11.0 / 3, -2.0, 1.0, -2.0, 1.0}));
    }
    SECTION("LU decomposition")
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        Matrix B = A.LU();
        REQUIRE(B == Matrix(3, 3, {1, 4, 7, 2, -3, -6, 3, 2, 1}));
    }
    SECTION("Cholesky decomposition")
    {
        Matrix A(4, 4, {4, -2, 4, 2, -2, 10, -2, -7, 4, -2, 8, 4, 2, -7, 4, 7});
        Matrix B = A.Cholesky();
        REQUIRE(B == Matrix(4, 4, {2, -1, 2, 1, -1, 3, 0, -2, 2, 0, 2, 1, 1, -2, 1, 1}));

        Matrix C = A.CholeskyD();
        REQUIRE(C == Matrix(4, 4, {4, -0.5, 1, 0.5, -0.5, 9, 0, -2.0 / 3, 1, 0, 4, 0.5, 0.5, -2.0 / 3, 0.5, 1}));
    }
    // 其余函数通过测试求解器间接验证
}

TEST_CASE("Test Solver")
{
    SECTION("Lower Triangular solver")
    {
        Matrix A(3, 3, {1, 0, 0, 2, 5, 0, 3, 6, 10});
        Vector b({1, 3, 4});
        LinearSolver solver;
        Vector x = solver.solveLower(A, b);
        REQUIRE(x == Vector({1, 0.2, -0.02}));

        x = solver.solveLower(A, b, true);
        REQUIRE(x == Vector({1, 1, -5}));
    }
    SECTION("Upper Triangular solver")
    {
        Matrix A(3, 3, {1, 3, 4, 0, 5, 6, 0, 0, 10});
        Vector b({1, 2, 4});
        LinearSolver solver;
        Vector x = solver.solveUpper(A, b);
        REQUIRE(x == Vector({-0.36, -0.08, 0.4}));

        x = solver.solveUpper(A, b, true);
        REQUIRE(x == Vector({51, -22, 4}));
    }
    SECTION("Basic solver")
    {
        Matrix A(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
        Vector b({1, 2, 4});
        LinearSolver solver;
        Vector x = solver.solve(A, b);
        REQUIRE(x == Vector({2.0, -2.0, 1.0}));

        x = solver.solve(A, b, true);
        REQUIRE(x == Vector({2.0, -2.0, 1.0}));
    }
    SECTION("Symmetric solver")
    {
        Matrix A(4, 4, {4, -2, 4, 2, -2, 10, -2, -7, 4, -2, 8, 4, 2, -7, 4, 7});
        Vector b({8, -1, 14, 6});
        LinearSolver solver;
        Vector x = solver.Cholesky(A, b);
        REQUIRE(x == Vector({1, 1, 1, 1}));

        x = solver.Cholesky(A, b, true);
        REQUIRE(x == Vector({1, 1, 1, 1}));

        x = solver.solve(A, b);
        REQUIRE(x == Vector({1, 1, 1, 1}));

        x = solver.solve(A, b, true);
        REQUIRE(x == Vector({1, 1, 1, 1}));

        x = solver.CG(A, b);
        REQUIRE(x == Vector({1, 1, 1, 1}));
    }
    SECTION("Jacobi Iteration")
    {
        Matrix A(3, 3, {1, 2, -2, 1, 1, 1, 2, 2, 1});
        Vector b({-1, 6, 9});
        LinearSolver solver;
        Vector x = solver.Jacobi(A, b);
        REQUIRE(x == Vector({1, 2, 3}));
    }
    SECTION("GS Iteration")
    {
        Matrix A(3, 3, {2, -1, 1, 1, 1, 1, 1, 1, -2});
        Vector b({3, 6, -3});
        LinearSolver solver;
        Vector x = solver.GaussSediel(A, b);
        REQUIRE(x == Vector({1, 2, 3}));
    }
    SECTION("SOR Iteration")
    {
        Matrix A(3, 3, {2, -1, 1, 1, 1, 1, 1, 1, -2});
        Vector b({3, 6, -3});
        LinearSolver solver;
        Vector x = solver.SOR(A, b, 0.8);
        REQUIRE(x == Vector({1, 2, 3}));

        x = solver.SOR(A, b, 1.2);
        REQUIRE(x == Vector({1, 2, 3}));
    }
    SECTION("LSQ solver")
    {
        Matrix A(7, 4, {1, 2, -2, 5, 1, 1, 1, 4, 2, 2, 2, 9, 2, 3, 4, 5, 6, 8, 9, -5, 2, 3, 4, 5, 6, 7, 9, -5});
        Vector b({5, 6, 7, 8, 1, 2, 4});
        LinearSolver solver;
        Vector x = solver.QR(A, b);
        REQUIRE(x == Vector({2.479465943383723, -0.880183882547382, -0.264909586025962, 0.665423885233720}));
    }
    SECTION("Power solver")
    {
        Matrix A(5, 5, {-15, 85, -225, 274, -120, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0});
        LinearSolver solver;
        Approx target = Approx(solver.power(A, 1)).epsilon(0.01);
        REQUIRE(target == -5.000000000000059);
    }
    SECTION("Inverse Power Eigen solver")
    {
        Matrix A(5, 5, {-15, 85, -225, 274, -120, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, -1, 0});
        LinearSolver solver;
        Vector x = solver.inversePower(A, -5);
        REQUIRE(x.abs() == Vector({0.979795947278826, 0.195959189455760, 0.039191837891151, 0.007838367578230, 0.001567673515646}));
    }
    SECTION("Implicit QR solver")
    {
        Matrix A(6, 6, {1.1908, -1.0565, -2.1707, 0.5913, 0, 0.7310, -1.2025, 1.4151, -0.0592, -0.6436, -0.3179, 0.5779, -0.0198, -0.8051, -1.0106, 0.3803, 1.0950, 0.0403, -0.1567, 0.5287, 0.6145, -1.0091, -1.8740, 0.6771, -1.6041, 0.2193, 0.5077, -0.0195, 0.4282, 0.5689, 0.2573, -0.9219, 1.6924, -0.0482, 0.8956, -0.2556});
        LinearSolver solver;
        Matrix Q(6, 6);
        for (int i = 0; i < 6; i++)
            Q(i, i) = 1;

        // 验证 Hessenberg
        Matrix H = A.Hessenberg(&Q);
        REQUIRE(A == Matrix(Q * H * Q.transpose()));

        Matrix P(6, 6);
        for (int i = 0; i < 6; i++)
            P(i, i) = 1;
        Matrix S = solver.implicitQR(A, &P);

        // 由于 QR 方法过程中去除了很多相对小的元素，导致最终计算结果会有一定误差
        CHECK(A == Matrix(P * S * P.transpose()));
    }
    SECTION("Bisection solver")
    {
        // 针对三对角阵
        Matrix A(4, 4, {1, 1, 0, 0, 1, 2, 3, 0, 0, 3, 6, 10, 0, 0, 10, 20});
        LinearSolver solver;
        Vector lambda = solver.bisection(A);
        REQUIRE(lambda == Vector({-1.523734777993481, 0.974969844946525, 4.258610702322208, 25.290154230724745}));
    }
    SECTION("passJacobi solver")
    {
        Matrix A(4, 4, {1, 1, 1, 1, 1, 2, 3, 4, 1, 3, 6, 10, 1, 4, 10, 20});
        LinearSolver solver;
        Matrix Q = solver.passJacobi(A, 10);
        REQUIRE(Q == Matrix(4, 4, {0.787275376005945, -0.308686320226581, 0.530365719772128, 0.060186720547497, -0.163233650892682, 0.723090316194097, 0.640331730884963, 0.201172672757594, -0.532106691666762, -0.594550779583976, 0.391832131205882, 0.458082328991412, 0.265357732569444, 0.168411759765698, -0.393897269179563, 0.863752102325145}));
        REQUIRE(A.diag() == Vector({0.453834550025667, 0.038016015229141, 2.203446167647325, 26.304703267097874}));
    }
    SECTION("Symmetric Eigen solver")
    {
        Matrix A(4, 4, {1, 1, 1, 1, 1, 2, 3, 4, 1, 3, 6, 10, 1, 4, 10, 20});
        LinearSolver solver;
        Matrix Q(4, 4);
        for (int i = 0; i < 4; i++)
            Q(i, i) = 1;
        Matrix S = solver.implicitQR(A, &Q, true);
        CHECK(Q == Matrix(4, 4, {0.060186720547497, -0.530365719772128, 0.787275376005945, -0.308686320226581, 0.201172672757594, -0.640331730884963, -0.163233650892682, 0.723090316194097, 0.458082328991412, -0.391832131205882, -0.532106691666762, -0.594550779583976, 0.863752102325145, 0.393897269179563, 0.265357732569444, 0.168411759765698}));
        REQUIRE(S.diag() == Vector({26.304703267097874, 2.203446167647325, 0.453834550025667, 0.038016015229141}));
    }
    SECTION("SVD solver")
    {
        Matrix A(7, 4, {1, 2, -2, 5, 1, 1, 1, 4, 2, 2, 2, 9, 2, 3, 4, 5, 6, 8, 9, -5, 2, 3, 4, 5, 6, 7, 9, -5});
        LinearSolver solver;

        // 注意先设为对角阵
        Matrix U(7, 7);
        for (int i = 0; i < 7; i++)
            U(i, i) = 1;
        Matrix V(4, 4);
        for (int i = 0; i < 4; i++)
            V(i, i) = 1;

        Matrix S = solver.SVD(A, &U, &V);
        CHECK(S == Matrix(7, 4, {20.647924190113297, 0, 0, 0, 0, 14.679570743204255, 0, 0, 0, 0, 0.764867795802106, 0, 0, 0, 0, 2.930598350088416, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}));
        REQUIRE(A == Matrix(U * S * V.transpose()));
    }
}

TEST_CASE("Test SpMatrix")
{
    SECTION("Test Initalization")
    {
        srand(time(0));

        Matrix A(8, 4);
        SpMatrix SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.set(i, j, value);
        }

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(A(i, j) == SA(i, j));

        Matrix B(4, 8);
        SpMatrix SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB.set(j, i, value);
        }

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(B(j, i) == SB(j, i));

        REQUIRE(SA.row() == 8);
        REQUIRE(SA.col() == 4);
        REQUIRE(SA.nnz() == 0);
    }
    SECTION("Test +-")
    {
        srand(time(0));

        Matrix A(8, 4);
        SpMatrix SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.set(i, j, value);
        }

        Matrix B(8, 4);
        SpMatrix SB(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(i, j) = value;
            SB.set(i, j, value);
        }

        Matrix C = A + B;
        SpMatrix SC = SA + SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(C(i, j) == SC(i, j));

        C = A - B;
        SC = SA - SB;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(C(i, j) == SC(i, j));
    }
    SECTION("Test */")
    {
        srand(time(0));

        Matrix A(8, 4);
        SpMatrix SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.set(i, j, value);
        }

        Matrix C = A / 3;
        SpMatrix SC = SA / 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(C(i, j) == SC(i, j));

        C = A * 3;
        SC = SA * 3;

        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(C(i, j) == SC(i, j));
    }
    SECTION("Test Multiplication")
    {
        srand(time(0));

        Matrix A(8, 4);
        SpMatrix SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.set(i, j, value);
        }

        Matrix B(4, 8);
        SpMatrix SB(4, 8);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            B(j, i) = value;
            SB.set(j, i, value);
        }

        Matrix C = A * B;
        SpMatrix SC = SA * SB;
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 8; j++)
                REQUIRE(C(i, j) == SC(i, j));

        Matrix D = B * A;
        SpMatrix SD = SB * SA;
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(D(i, j) == SD(i, j));
    }
    SECTION("Test toMatrix")
    {
        srand(time(0));

        Matrix A(8, 4);
        SpMatrix SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(i, j) = value;
            SA.set(i, j, value);
        }

        Matrix B = SA.toMatrix();
        for (int i = 0; i < 8; i++)
            for (int j = 0; j < 4; j++)
                REQUIRE(A(i, j) == B(i, j));
    }
    SECTION("Test transpose")
    {
        srand(time(0));

        Matrix A(4, 8);
        SpMatrix SA(8, 4);
        for (int k = 0; k < 16; k++)
        {
            int i = rand() % 8;
            int j = rand() % 4;
            int value = rand() % 100;
            A(j, i) = value;
            SA.set(i, j, value);
        }

        SpMatrix B = SA.transpose();
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 8; j++)
                REQUIRE(A(i, j) == B(i, j));
    }
}

TEST_CASE("Test Vector3f")
{
    SECTION("Test operations")
    {
        Vector3f v(1.2, 3.4, 5.6);
        Vector3f v2(3.4, 5.6, 7.8);
        REQUIRE((v + v2) == Vector3f(4.6, 9.0, 13.4));
        REQUIRE((v2 - v) == Vector3f(2.2, 2.2, 2.2));
        REQUIRE((v * 2) == Vector3f(2.4, 6.8, 11.2));
        REQUIRE((v / 2) == Vector3f(0.6, 1.7, 2.8));
    }
    SECTION("Test algorithm")
    {
        Vector3f v(1, 3, 5);
        Vector3f v2(3, 5, 7);
        Vector3f v3(1, 5, 2);
        Vector3f v4(5, 7, 9);
        Vector3f v5(6, 2, 3);
        REQUIRE(linear(v, v2, 0.2) == Vector3f(0.8 + 0.6, 2.4 + 1.0, 4.0 + 1.4));
        REQUIRE(cross(v, v2) == Vector3f(-4, 8, -4));
        REQUIRE(normalize(v) == Vector3f(1 / sqrt(35), 3 / sqrt(35), 5 / sqrt(35)));
        REQUIRE(det(v, v2, v3) == 28);

        REQUIRE(Approx(length(v)).epsilon(0.01) == sqrt(35));
        REQUIRE(Approx(distance(v, v2)).epsilon(0.01) == (sqrt(3) * 2));
        REQUIRE(Approx(dot(v, v2)).epsilon(0.01) == 53);
        REQUIRE(onLine(v, v2, v3) == false);
        REQUIRE(onLine(v, v2, v4) == true);
        REQUIRE(onFace(v, v2, v3, v4) == true);
        REQUIRE(onFace(v, v2, v3, v5) == false);
    }
}

TEST_CASE("Test Quaternion")
{
    SECTION("Test operations")
    {
        Quaternion qa(2, {-2, 3, -4});
        Quaternion qb(1, {-2, 5, -6});
        REQUIRE((qa + qb) == Quaternion(3, {-4, 8, -10}));
        REQUIRE((qa - qb) == Quaternion(1, {0, -2, 2}));
        REQUIRE((qa * 2) == Quaternion(4, {-4, 6, -8}));
        REQUIRE((qa / 2) == Quaternion(1, {-1, 1.5, -2}));
        REQUIRE(Approx(length(qa)).epsilon(0.01) == sqrt(33));
        REQUIRE(Approx(length(qb)).epsilon(0.01) == sqrt(66));
        REQUIRE(normalize(qa) == (qa / sqrt(33)));
        REQUIRE(normalize(qb) == (qb / sqrt(66)));
        REQUIRE((qa * qb) == Quaternion(-41, {-4, 9, -20}));
        REQUIRE((qb * qa) == Quaternion(-41, {-8, 17, -12}));
        REQUIRE((Quaternion(1, {0, 0, 0}) / qa) == (Quaternion(2, {2, -3, 4}) / 33));
    }
    SECTION("Test rotate")
    {
        const double PI = 3.1415926;
        Vector3f v(1 / sqrt(2), 0, 1 / sqrt(2));
        Quaternion q(v, PI / 2);
        Quaternion p(0, {2, 0, 0});
        // 逆时针绕 v 转 90 度
        REQUIRE((q * p / q) == Quaternion(0, {1, sqrt(2), 1}));

        // 从一个轴转到另一个轴
        Vector3f v1({2.4, 3.6, 5.8});
        v1 = normalize(v1);
        Vector3f v2({5.1, 9.6, -2.7});
        v2 = normalize(v2);
        Quaternion q2(v1, v2);
        REQUIRE((q2 * Quaternion(0, v1) / q2) == Quaternion(0, v2));
    }
}

TEST_CASE("Test Basis")
{
    SECTION("Test FindSpan")
    {
        SECTION("Test FindSpan1")
        {
            MiniArray<Vector3f> points(8);
            MiniArray<Real> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

            MiniBSplineCurve spline(points, knots, 2);
            REQUIRE(FindSpan(0.0, spline.GetDegree(), spline.GetKnots()) == 2);
            REQUIRE(FindSpan(0.5, spline.GetDegree(), spline.GetKnots()) == 2);
            REQUIRE(FindSpan(1.5, spline.GetDegree(), spline.GetKnots()) == 3);
            REQUIRE(FindSpan(2.5, spline.GetDegree(), spline.GetKnots()) == 4);
            REQUIRE(FindSpan(3.5, spline.GetDegree(), spline.GetKnots()) == 5);
            REQUIRE(FindSpan(4.5, spline.GetDegree(), spline.GetKnots()) == 7);
            REQUIRE(FindSpan(5.0, spline.GetDegree(), spline.GetKnots()) == 7);
        }
        SECTION("Test FindSpan2")
        {
            MiniArray<Vector3f> points(8);
            MiniArray<Real> knots({0, 0, 0, 1, 2, 2, 4, 4, 5, 5, 5});

            MiniBSplineCurve spline(points, knots, 2);
            REQUIRE(FindSpan(0.0, spline.GetDegree(), spline.GetKnots()) == 2);
            REQUIRE(FindSpan(0.5, spline.GetDegree(), spline.GetKnots()) == 2);
            REQUIRE(FindSpan(1.5, spline.GetDegree(), spline.GetKnots()) == 3);
            REQUIRE(FindSpan(2.5, spline.GetDegree(), spline.GetKnots()) == 5);
            REQUIRE(FindSpan(3.5, spline.GetDegree(), spline.GetKnots()) == 5);
            REQUIRE(FindSpan(4.5, spline.GetDegree(), spline.GetKnots()) == 7);
            REQUIRE(FindSpan(5.0, spline.GetDegree(), spline.GetKnots()) == 7);
        }
    }
    SECTION("Test FindSpanMult")
    {
        MiniArray<Vector3f> points(8);
        MiniArray<Real> knots({0, 0, 0, 1, 2, 4, 4, 4, 5, 5, 5});

        MiniBSplineCurve spline(points, knots, 2);
        int k = FindSpan(2.5, spline.GetDegree(), spline.GetKnots());
        REQUIRE(FindSpanMult(k, 2.5, spline.GetDegree(), spline.GetKnots()) == 0);
        k = FindSpan(2.0, spline.GetDegree(), spline.GetKnots());
        REQUIRE(FindSpanMult(k, 2.0, spline.GetDegree(), spline.GetKnots()) == 1);
        k = FindSpan(4.0, spline.GetDegree(), spline.GetKnots());
        REQUIRE(FindSpanMult(k, 4.0, spline.GetDegree(), spline.GetKnots()) == 3);
        k = FindSpan(0.0, spline.GetDegree(), spline.GetKnots());
        REQUIRE(FindSpanMult(k, 0.0, spline.GetDegree(), spline.GetKnots()) == 1);
    }
    SECTION("Test BasisFuncs")
    {
        MiniArray<Vector3f> points(8);
        MiniArray<Real> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

        MiniBSplineCurve spline(points, knots, 2);
        MiniArray<Real> N = BasisFuncs(4, 2.5, spline.GetDegree(), spline.GetKnots());
        REQUIRE(N == MiniArray<Real>{1.0 / 8, 6.0 / 8, 1.0 / 8});
    }
    SECTION("Test AllBasisFuncs")
    {
        MiniArray<Vector3f> points(8);
        MiniArray<Real> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

        MiniBSplineCurve spline(points, knots, 2);
        MiniArray2D<Real> N(3);
        N[0] = MiniArray<Real>({1});
        N[1] = MiniArray<Real>({0.5, 0.5});
        N[2] = MiniArray<Real>({1.0 / 8, 6.0 / 8, 1.0 / 8});
        REQUIRE(AllBasisFuncs(4, 2.5, spline.GetDegree(), spline.GetKnots()) == N);
    }
    SECTION("Test DerivBasisFuncs")
    {
        MiniArray<Vector3f> points(8);
        MiniArray<Real> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

        MiniBSplineCurve spline(points, knots, 2);
        int span = FindSpan(2.5, spline.GetDegree(), spline.GetKnots());
        REQUIRE(span == 4);
        REQUIRE(DerivBasisFuncs(2, span, 2.5, spline.GetDegree(), spline.GetKnots()) == MiniArray2D<Real>({{0.125, 0.75, 0.125}, {-0.5, 0.0, 0.5}, {1.0, -2.0, 1.0}}));
    }
}

TEST_CASE("Test B-Spline")
{
    SECTION("Test Get")
    {
        MiniArray<Vector3f> points(8);
        for (int i = 0; i < 8; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        MiniArray<Real> knots({0.1, 0.2, 0.3, 1, 2, 3, 4, 4, 5.1, 5.2, 5.3});

        MiniBSplineCurve spline(points, knots, 2);
        REQUIRE(spline.GetPointsNum() == 8);
        REQUIRE(spline.GetPoints() == points);
        REQUIRE(spline.GetDegree() == 2);
        REQUIRE(spline.GetKnots() == knots);
        REQUIRE(spline.GetPeriodic() == false);
        REQUIRE(spline.GetPoints() == points);
        REQUIRE(spline.GetBegin() == 0.3);
        REQUIRE(spline.GetEnd() == 5.1);
    }
    SECTION("Test CurvePoint")
    {
        MiniArray<Vector3f> points(8);
        for (int i = 0; i < 8; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        MiniArray<Real> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

        MiniBSplineCurve spline(points, knots, 2);
        REQUIRE(spline.CurvePoint(2.5) == (points[2] / 8 + points[3] * 6 / 8 + points[4] / 8));
    }
    SECTION("Test CurveDerivCpts1")
    {
        int m = 7;
        int p = 2;
        int n = m - p - 1;
        MiniArray<Vector3f> points(5);
        for (int i = 0; i < 5; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        MiniArray<Real> knots({0, 0, 0, 0.4, 0.6, 1, 1, 1});

        MiniBSplineCurve spline(points, knots, p);
        // 求导后只有 4 个有效的控制点，剩下的为零（空）
        MiniArray<Vector3f> derivPoints(5);
        derivPoints[0] = (points[1] - points[0]) * 5;
        derivPoints[1] = (points[2] - points[1]) * 10 / 3;
        derivPoints[2] = (points[3] - points[2]) * 10 / 3;
        derivPoints[3] = (points[4] - points[3]) * 5;
        MiniArray2D<Vector3f> PK = spline.CurveDerivCpts(1, 0, n);
        REQUIRE(PK[0] == points);
        REQUIRE(PK[1] == derivPoints);
    }
    SECTION("Test CurveDerivCpts2")
    {
        int m = 10;
        int p = 3;
        int n = m - p - 1;
        MiniArray<Vector3f> points(7);
        for (int i = 0; i < 7; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        MiniArray<Real> knots({0, 0, 0, 0, 1.0 / 3, 2.0 / 3, 2.0 / 3, 1, 1, 1, 1});

        MiniBSplineCurve spline(points, knots, p);
        // 求导后只有 6 个有效的控制点，剩下的为零（空）
        MiniArray<Vector3f> derivPoints(7);
        derivPoints[0] = (points[1] - points[0]) * 9;
        derivPoints[1] = (points[2] - points[1]) * 4.5;
        derivPoints[2] = (points[3] - points[2]) * 4.5;
        derivPoints[3] = (points[4] - points[3]) * 4.5;
        derivPoints[4] = (points[5] - points[4]) * 9;
        derivPoints[5] = (points[6] - points[5]) * 9;
        MiniArray2D<Vector3f> PK = spline.CurveDerivCpts(1, 0, n);
        REQUIRE(PK[0] == points);
        REQUIRE(PK[1] == derivPoints);
    }
    SECTION("Test DerivCurvePoint")
    {
        int m = 10;
        int p = 2;
        int n = m - p - 1;
        MiniArray<Vector3f> points(8);
        for (int i = 0; i < 8; i++)
            points[i] = {i * 0.5, i * i * 0.8, sin(i)};
        MiniArray<Real> knots({0, 0, 0, 1, 2, 3, 4, 4, 5, 5, 5});

        MiniBSplineCurve spline(points, knots, p);
        MiniArray<Vector3f> CK = spline.CurveDerivPoint(1, 2.5);
        REQUIRE(CK[1] == (points[2] * -0.5 + points[4] * 0.5));
    }
    SECTION("Test knotInsert")
    {
        auto f = [](float t) -> Vector3f
        {
            return Vector3f{0.0005 * t * t, 5 * sin(t), 0.05 * t};
        };

        MiniArray<Vector3f> points5(9);
        for (int i = 0; i < 9; i++)
            points5[i] = f(10 * i);
        MiniArray<Real> knots2({-2, -1, 0, 0.8, 1, 2, 3, 4, 4.5, 5, 6, 7, 8});
        MiniBSplineCurve *spline = new MiniBSplineCurve(points5, knots2, 3);
        spline->CurveKnotIns(2.5, 2);

        MiniBSplineCurve *spline2 = new MiniBSplineCurve(points5, knots2, 3);
        Real a = spline2->GetBegin();
        Real b = spline2->GetEnd();
        Real step = (b - a) / 999;
        for (int i = 0; i < 1000; i++)
        {
            Real u = a + step * i;
            REQUIRE(spline->CurvePoint(u) == spline2->CurvePoint(u));
        }
    }
    SECTION("Test knotRefinement")
    {
        auto f = [](float t) -> Vector3f
        {
            return Vector3f{0.0005 * t * t, 5 * sin(t), 0.05 * t};
        };

        MiniArray<Vector3f> points4(8);
        for (int i = 0; i < 8; i++)
        {
            points4[i] = f(10 * i);
            points4[i].rx() *= 5;
            points4[i].rx() -= 5;
        }
        MiniArray<Real> knots({-1, 0, 0.5, 1, 2, 3, 4.2, 4.5, 5, 6, 7});

        MiniArray<Vector3f> points5(9);
        for (int i = 0; i < 9; i++)
        {
            points5[i] = f(10 * i);
            points5[i].rx() *= 5;
            points5[i].rx() -= 5;
            points5[i].rz() -= 2;
        }
        MiniArray<Real> knots2({-2, -1, 0, 0.8, 1, 2, 3, 4, 4.5, 5, 6, 7, 8});

        MiniBSplineCurve *spline = new MiniBSplineCurve(points5, knots2, 3);
        spline->CurveRefineKnot({0.9, 1.6, 2.5, 4.3, 4.3, 4.6});

        MiniBSplineCurve *spline2 = new MiniBSplineCurve(points5, knots2, 3);
        Real a = spline2->GetBegin();
        Real b = spline2->GetEnd();
        Real step = (b - a) / 999;
        for (int i = 0; i < 1000; i++)
        {
            Real u = a + step * i;
            REQUIRE(spline->CurvePoint(u) == spline2->CurvePoint(u));
        }
    }
    SECTION("Test Projection")
    {
        auto f = [](float t) -> Vector3f
        {
            return Vector3f{cos(t) * t, sin(2 * t) + 5, sin(t) * t * t / 5};
        };

        // 64 个采样点
        int m = 63;
        MiniArray<Vector3f> points4(m + 1);
        const Real PI = 3.1415926535;
        MiniArray<Real> s(m + 1);
        for (int i = 0; i < m + 1; i++)
        {
            s[i] = 2 * PI / m * i;
            points4[i] = f(2 * PI / m * i);
        }
        // 输入参数和对应的点
        MiniDiscreteCurve *curve = new MiniDiscreteCurve(points4, s);

        int N = 11;
        int d = 3;
        MiniBSplineCurve *spline = curve->FittingCurve(N, d);
        delete curve;

        Vector3f P = {2.0, 2.0, -2.0};

        // 测试最近点投影
        Real u = spline->NearPointProjection(P, 0.00001);
        auto Q = spline->CurveDerivPoint(1, u);
        Real angle = dot(Q[1], Q[0] - P) / length(Q[1]) / length(Q[0] - P);
        REQUIRE(angle < 1e-6);

        // 测试正交投影
        u = spline->OrthoPointProjection(P, 0.00001);
        Q = spline->CurveDerivPoint(1, u);
        angle = dot(Q[1], Q[0] - P) / length(Q[1]) / length(Q[0] - P);
        REQUIRE(angle < 1e-6);

        delete spline;
    }
}

TEST_CASE("Test B-spline surface")
{
    SECTION("Test Get")
    {
        MiniArray2D<Vector3f> P2(5, MiniArray<Vector3f>(6));
        for (int i = 0; i < 5; i++)
            for (int j = 0; j < 6; j++)
                P2[i][j] = Vector3f(i, sin(i + j), j);

        MiniArray<Real> uknots({0, 0.1, 0.2, 0.4, 0.6, 1, 2, 3});
        MiniArray<Real> vknots({0, 0.1, 0.15, 0.2, 0.5, 0.8, 1, 2, 3, 3});
        MiniBSplineSurface surface(P2, uknots, vknots, 2, 3, false, true);
        REQUIRE(surface.GetUBegin() == 0.2);
        REQUIRE(surface.GetVBegin() == 0.2);
        REQUIRE(surface.GetUEnd() == 1);
        REQUIRE(surface.GetVEnd() == 1);
        REQUIRE(surface.GetUDegree() == 2);
        REQUIRE(surface.GetVDegree() == 3);
        REQUIRE(surface.GetUPeriodic() == false);
        REQUIRE(surface.GetVPeriodic() == true);
        REQUIRE(surface.GetUPointsNum() == 5);
        REQUIRE(surface.GetVPointsNum() == 6);
        REQUIRE(surface.GetUKnots() == uknots);
        REQUIRE(surface.GetVKnots() == vknots);
        REQUIRE(surface.GetPoints() == P2);
    }
    SECTION("Test SurfacePoint")
    {
        MiniArray2D<Vector3f> P2(5, MiniArray<Vector3f>(6));
        for (int i = 0; i < 5; i++)
            for (int j = 0; j < 6; j++)
                P2[i][j] = Vector3f(i, sin(i + j), j);

        MiniArray<Real> uknots({0, 0.1, 0.2, 0.4, 0.6, 1, 2, 3});
        MiniArray<Real> vknots({0, 0.1, 0.15, 0.2, 0.5, 0.8, 1, 2, 3, 3});
        MiniBSplineSurface surface(P2, uknots, vknots, 2, 3, false, false);

        // 先算两个方向的非零基函数
        int ui = FindSpan(0.3, surface.GetUDegree(), surface.GetUKnots());
        MiniArray<Real> uB = BasisFuncs(ui, 0.3, surface.GetUDegree(), surface.GetUKnots());

        int vi = FindSpan(0.6, surface.GetVDegree(), surface.GetVKnots());
        MiniArray<Real> vB = BasisFuncs(vi, 0.6, surface.GetVDegree(), surface.GetVKnots());

        // 组合利用定义计算点
        Vector3f S;
        for (int i = 0; i < 3; i++)
        {
            Vector3f tmp;
            for (int j = 0; j < 4; j++)
                tmp = tmp + P2[ui - 2 + i][vi - 3 + j] * vB[j];
            S = S + tmp * uB[i];
        }

        // 计算曲面上的点，与定义计算的结果比较
        REQUIRE(surface.SurfacePoint(0.3, 0.6) == S);
    }
    SECTION("Test SurfaceDerivPoint")
    {
        MiniArray2D<Vector3f> P2(5, MiniArray<Vector3f>(6));
        for (int i = 0; i < 5; i++)
            for (int j = 0; j < 6; j++)
                P2[i][j] = Vector3f(i, sin(i + j), j);

        MiniArray<Real> uknots({0, 0.1, 0.2, 0.4, 0.4, 1, 2, 3});
        MiniArray<Real> vknots({0, 0.1, 0.1, 0.2, 0.5, 0.8, 1, 2, 3, 3});
        MiniBSplineSurface surface(P2, uknots, vknots, 2, 3, false, false);

        MiniArray<Vector3f> points1(5);
        MiniBSplineCurve spline1(points1, uknots, 2);

        MiniArray<Vector3f> points2(6);
        MiniBSplineCurve spline2(points2, vknots, 3);

        float bu = surface.GetUBegin();
        float eu = surface.GetUEnd();
        float bv = surface.GetVBegin();
        float ev = surface.GetVEnd();

        for (int i = 0; i < 100; i++)
        {
            for (int j = 0; j < 100; j++)
            {
                float u = bu + (eu - bu) / 99 * i;
                float v = bv + (ev - bv) / 99 * j;

                int ui = FindSpan(u, surface.GetUDegree(), surface.GetUKnots());
                int vi = FindSpan(v, surface.GetVDegree(), surface.GetVKnots());

                MiniArray2D<Real> Dp1 = DerivBasisFuncs(2, ui, u, spline1.GetDegree(), spline1.GetKnots());
                MiniArray2D<Real> Dp2 = DerivBasisFuncs(2, vi, v, spline2.GetDegree(), spline2.GetKnots());

                // 输入求导后的基函数，计算导数曲面上的点
                auto f = [=](MiniArray<Real> p1, MiniArray<Real> p2) -> Vector3f
                {
                    Vector3f p;
                    for (int i = 0; i <= 2; i++)
                    {
                        Vector3f tmp;
                        for (int j = 0; j <= 3; j++)
                            tmp = tmp + P2[ui - 2 + i][vi - 3 + j] * p2[j];
                        p = p + tmp * p1[i];
                    }
                    return p;
                };

                // 求曲面上的点
                Vector3f p = surface.SurfacePoint(u, v);

                // 求 2 阶导对应的点
                MiniArray2D<Vector3f> P = surface.SurfaceDerivPoint(2, u, v);

                // 验证所有偏导
                REQUIRE(P[0][0] == p);
                REQUIRE(P[0][0] == f(Dp1[0], Dp2[0]));
                REQUIRE(P[1][0] == f(Dp1[1], Dp2[0]));
                REQUIRE(P[0][1] == f(Dp1[0], Dp2[1]));
                REQUIRE(P[1][1] == f(Dp1[1], Dp2[1]));
                REQUIRE(P[2][0] == f(Dp1[2], Dp2[0]));
                REQUIRE(P[0][2] == f(Dp1[0], Dp2[2]));
            }
        }
    }
    SECTION("Test knotInsert")
    {
        MiniArray2D<Vector3f> P2(5, MiniArray<Vector3f>(6));
        for (int i = 0; i < 5; i++)
            for (int j = 0; j < 6; j++)
                P2[i][j] = Vector3f(i, sin(i + j), j);

        MiniArray<Real> uknots2({0, 0.1, 0.2, 0.4, 0.6, 1, 2, 3});
        MiniArray<Real> vknots2({0, 0.1, 0.15, 0.2, 0.5, 0.8, 1, 2, 3, 3});

        MiniBSplineSurface *spline = new MiniBSplineSurface(P2, uknots2, vknots2, 2, 3, false, false);
        spline->SurfaceKnotIns(0.3, 2, 'u');
        spline->SurfaceKnotIns(0.4, 2, 'v');

        MiniBSplineSurface *spline2 = new MiniBSplineSurface(P2, uknots2, vknots2, 2, 3, false, false);
        Real bu = spline2->GetUBegin();
        Real eu = spline2->GetUEnd();
        Real bv = spline2->GetVBegin();
        Real ev = spline2->GetVEnd();
        Real su = (eu - bu) / 99;
        Real sv = (ev - bv) / 99;
        for (int i = 0; i < 100; i++)
        {
            Real u = bu + su * i;
            for (int j = 0; j < 100; j++)
            {
                Real v = bv + sv * j;
                REQUIRE(spline->SurfacePoint(u, v) == spline2->SurfacePoint(u, v));
            }
        }
    }
    SECTION("Test knotRefinement")
    {
        MiniArray2D<Vector3f> P2(5, MiniArray<Vector3f>(6));
        for (int i = 0; i < 5; i++)
            for (int j = 0; j < 6; j++)
                P2[i][j] = Vector3f(i, sin(i + j), j);

        MiniArray<Real> uknots2({0, 0.1, 0.2, 0.4, 0.6, 1, 2, 3});
        MiniArray<Real> vknots2({0, 0.1, 0.15, 0.2, 0.5, 0.8, 1, 2, 3, 3});

        MiniBSplineSurface *spline = new MiniBSplineSurface(P2, uknots2, vknots2, 2, 3, false, false);
        spline->SurfaceRefineKnot({0.3, 0.5, 0.5, 0.7, 0.8, 0.9}, 'u');
        spline->SurfaceRefineKnot({0.2, 0.3, 0.4, 0.6, 0.6, 0.7, 0.9}, 'v');

        MiniBSplineSurface *spline2 = new MiniBSplineSurface(P2, uknots2, vknots2, 2, 3, false, false);
        Real bu = spline2->GetUBegin();
        Real eu = spline2->GetUEnd();
        Real bv = spline2->GetVBegin();
        Real ev = spline2->GetVEnd();
        Real su = (eu - bu) / 99;
        Real sv = (ev - bv) / 99;
        for (int i = 0; i < 100; i++)
        {
            Real u = bu + su * i;
            for (int j = 0; j < 100; j++)
            {
                Real v = bv + sv * j;
                REQUIRE(spline->SurfacePoint(u, v) == spline2->SurfacePoint(u, v));
            }
        }
    }
    SECTION("Test Projection")
    {
        auto f = [](float u, float v) -> Vector3f
        {
            return Vector3f{u, sin(u + v) * cos(u - v), v};
        };

        // 20 x 30 个采样点
        int um = 20;
        int vm = 30;
        MiniArray<Vector3f> points((um + 1) * (vm + 1));
        const Real PI = 3.1415926535;
        MiniArray<Real> u((um + 1) * (vm + 1));
        MiniArray<Real> v((um + 1) * (vm + 1));
        for (int i = 0; i < um + 1; i++)
        {
            for (int j = 0; j < vm + 1; j++)
            {
                // 记录每个点的 u,v 参数值
                int index = i * (vm + 1) + j;
                u[index] = 10.0 / um * i;
                v[index] = 10.0 / vm * j;
                points[i * (vm + 1) + j] = f(u[index], v[index]);
            }
        }
        // 输入参数和对应的点
        MiniCloud *cloud = new MiniCloud(points);

        int Nu = 10;
        int Nv = 10;
        int p = 3;
        int q = 3;
        MiniBSplineSurface *spline = cloud->FittingSurface(u, v, Nu, Nv, p, q);

        delete cloud;

        Vector3f P = {2.0, 0, 6.0};

        // 测试最近投影，用中心点参数作为初始迭代参数
        int index = 10 * (vm + 1) + 15;
        Vector P0 = {u[index], v[index]};
        auto uv = spline->NearPointProjection(P, P0, 0.00001);
        auto Q = spline->SurfaceDerivPoint(1, uv[0], uv[1]);
        auto N = cross(Q[0][1], Q[1][0]);
        auto V = Q[0][0] - P;
        Real angle = length(cross(N, V)) / (length(N) * length(V));
        REQUIRE(angle < 1e-6);

        uv = spline->OrthoPointProjection(P, P0, 0.00001);
        Q = spline->SurfaceDerivPoint(1, uv[0], uv[1]);
        N = cross(Q[0][1], Q[1][0]);
        V = Q[0][0] - P;
        angle = length(cross(N, V)) / (length(N) * length(V));
        REQUIRE(angle < 1e-5);

        delete spline;
    }
}

TEST_CASE("Test Cloud")
{
    SECTION("Test FittingCurve")
    {
        auto f = [](float t) -> Vector3f
        {
            return Vector3f{cos(t) * t, sin(2 * t) + 5, sin(t) * t * t / 5};
        };

        // 64 个采样点
        int m = 63;
        MiniArray<Vector3f> points(m + 1);
        const Real PI = 3.1415926535;
        MiniArray<Real> s(m + 1);
        for (int i = 0; i < m + 1; i++)
        {
            s[i] = 2 * PI / m * i;
            points[i] = f(2 * PI / m * i);
        }
        // 输入参数和对应的点
        MiniDiscreteCurve *curve = new MiniDiscreteCurve(points, s);
        Vector t = curve->GetPara();

        int N = 11;
        int d = 3;
        MiniBSplineCurve *spline = curve->FittingCurve(N, d);
        MiniBSplineCurve *spline2 = spline->LSPIACurve(N, d);
        delete curve;

        // 计算误差
        float err = 0;
        float err2 = 0;
        for (int i = 0; i < m + 1; i++)
        {
            double e = length(spline->CurvePoint(t[i]) - points[i]);
            err += e * e / (m + 1);

            e = length(spline2->CurvePoint(t[i]) - points[i]);
            err2 += e * e / (m + 1);
        }
        REQUIRE(err < 1e-8);
        REQUIRE(err2 < 1e-8);

        delete spline;
    }
    SECTION("Test FittingSurface")
    {
        auto f = [](float u, float v) -> Vector3f
        {
            return Vector3f{u, sin(u + v) * cos(u - v), v};
        };

        // 20 x 30 个采样点
        int um = 20;
        int vm = 30;
        MiniArray<Vector3f> points((um + 1) * (vm + 1));
        const Real PI = 3.1415926535;
        MiniArray<Real> u((um + 1) * (vm + 1));
        MiniArray<Real> v((um + 1) * (vm + 1));
        for (int i = 0; i < um + 1; i++)
        {
            for (int j = 0; j < vm + 1; j++)
            {
                // 记录每个点的 u,v 参数值
                int index = i * (vm + 1) + j;
                u[index] = 10.0 / um * i;
                v[index] = 10.0 / vm * j;
                points[i * (vm + 1) + j] = f(u[index], v[index]);
            }
        }
        // 输入参数和对应的点
        MiniCloud *cloud = new MiniCloud(points);

        int Nu = 15;
        int Nv = 15;
        int p = 3;
        int q = 3;
        MiniBSplineSurface *spline = cloud->FittingSurface(u, v, Nu, Nv, p, q);

        delete cloud;

        // 计算误差
        float err = 0;
        for (int i = 0; i < um + 1; i++)
        {
            for (int j = 0; j < vm + 1; j++)
            {
                int index = i * (vm + 1) + j;
                double e = length(spline->SurfacePoint(u[index], v[index]) - points[index]);
                err += e * e / (um + 1) / (vm + 1);
            }
        }
        REQUIRE(err < 1e-4);

        delete spline;
    }
}

#define EXP 2.718281828

double lambda = -100;
double eta = 1.5;
// double k = 0.01;
// double T = 1;

// // 需要注意 N = T / k 的计算方法可能不准
// int N = 100;

// 应该这样计算
double T = 1;
int N = 1000;
double k = T / N;

Vector f(Vector x, Real t)
{
    return Vector(1, lambda * (x[0] - cos(t)) - sin(t));
}

Real u(Real t)
{
    return pow(EXP, lambda * t) * (eta - 1) + cos(t);
}

TEST_CASE("Test FSolve")
{
    SECTION("Test Func11")
    {
        Func11 f = [](Real x) -> Real
        { return x * x - 1; };

        FSolve fS;
        Real x = fS(f, 0.5);
        REQUIRE(f(x) == 0);
    }

    SECTION("Test FuncXX")
    {
        FuncXX f = [](Vector x) -> Vector
        { return Vector({sin(x[0] + 0.5), cos(x[1] - 0.25)}); };

        FSolve fS;
        Vector x(2);
        fS(f, x);
        REQUIRE(f(x) == Vector({0, 0}));
    }
}

TEST_CASE("Test ODE")
{
    SECTION("Testing ClassicalRK")
    {
        ODESolver solver;
        ClassicalRK method(k);

        // 注意要多给一个
        TimeGrid G(N + 1, Vector(1));
        G[0] = {eta};

        solver.solve(f, G, &method);
        Approx target = Approx(G.back()[0]).epsilon(1e-8);
        REQUIRE(target == u(T));
    }

    SECTION("Testing ESDIRK")
    {
        ODESolver solver;
        ESDIRK method(k);
        TimeGrid G(N + 1, Vector(1));
        G[0] = {eta};

        solver.solve(f, G, &method);
        Approx target = Approx(G.back()[0]).epsilon(1e-6);
        REQUIRE(target == u(T));
    }

    SECTION("Testing GaussLegendre")
    {
        GaussLegendreRK method(3, k);
        ODESolver solver;
        TimeGrid G(N + 1, Vector(1));
        G[0] = {eta};

        solver.solve(f, G, &method);
        Approx target = Approx(G.back()[0]).epsilon(1e-8);
        REQUIRE(target == u(T));
    }
}